#include<stdio.h>
#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h> /* clock_gettime() */
#include <sys/mman.h> /* mlockall() */
#include <sched.h> /* sched_setscheduler() */

#include"ecrt.h"

/****************************************************************************/

/** Task period in ns. */
#define PERIOD_NS   (1000000)

#define MAX_SAFE_STACK (8 * 1024) /* The maximum stack size which is
                                     guranteed safe to access without
                                     faulting */

/****************************************************************************/

/* Constants */
#define NSEC_PER_SEC (1000000000)
#define FREQUENCY (NSEC_PER_SEC / PERIOD_NS)

/****************************************************************************/

// EtherCAT
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t *domainServoInput = NULL;
static ec_domain_state_t domainServoInput_state = {};
static ec_domain_t *domainServoOutput = NULL;
static ec_domain_state_t domainServoOutput_state = {};

static uint8_t *domainOutput_pd = NULL;
static uint8_t *domainInput_pd = NULL;

static ec_slave_config_t *sc_cooldrive = NULL;
static ec_slave_config_state_t sc_cooldrive_state = {};

/****************************************************************************/

// process data
#define CoolDrivePos0  1, 0
#define CoolDrivePos1  2, 1

#define CoolDrive 0x00000748, 0x0000000a


// offsets for PDO entries
static unsigned int  cntlwd1,cntlwd2,cntlwd3,cntlwd4;
static unsigned int  tarpos1,tarpos2,tarpos3,tarpos4;
static unsigned int  modper1,modper2,modper3,modper4;
static unsigned int  DB1_1,DB1_2,DB1_3,DB1_4;
static unsigned int  status1,status2,status3,status4;
static unsigned int  actpos1,actpos2,actpos3,actpos4;
static unsigned int  acttor1,acttor2,acttor3,acttor4;
static unsigned int  moddis1,moddis2,moddis3,moddis4;
static unsigned int  DB2_1,DB2_2,DB2_3,DB2_4;
static unsigned int  errcode1,errcode2,errcode3,errcode4;
static unsigned int  errflo1,errflo2,errflo3,errflo4;

static int cur_status;
static int cur_mode;
// process data
ec_pdo_entry_reg_t domainServoOutput_regs[] = {
    {CoolDrivePos0, CoolDrive, 0x6040, 0x00, &cntlwd1, NULL},
    {CoolDrivePos0, CoolDrive, 0x607a, 0x00, &tarpos1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6060, 0x00, &modper1, NULL},
    {CoolDrivePos0, CoolDrive, 0x27fe, 0x00, &DB1_1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6840, 0x00, &cntlwd2, NULL},
    {CoolDrivePos0, CoolDrive, 0x687a, 0x00, &tarpos2, NULL},
    {CoolDrivePos0, CoolDrive, 0x6860, 0x00, &modper2, NULL},
    {CoolDrivePos0, CoolDrive, 0x2ffe, 0x00, &DB1_2, NULL},
    {CoolDrivePos1, CoolDrive, 0x6040, 0x00, &cntlwd3, NULL},
    {CoolDrivePos1, CoolDrive, 0x607a, 0x00, &tarpos3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6060, 0x00, &modper3, NULL},
    {CoolDrivePos1, CoolDrive, 0x27fe, 0x00, &DB1_3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6840, 0x00, &cntlwd4, NULL},
    {CoolDrivePos1, CoolDrive, 0x687a, 0x00, &tarpos4, NULL},
    {CoolDrivePos1, CoolDrive, 0x6860, 0x00, &modper4, NULL},
    {CoolDrivePos1, CoolDrive, 0x2ffe, 0x00, &DB1_4, NULL},
    {}
};
ec_pdo_entry_reg_t domainServoInput_regs[] = {
    {CoolDrivePos0, CoolDrive, 0x6041, 0x00, &status1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6064, 0x00, &actpos1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6077, 0x00, &acttor1, NULL},    
    {CoolDrivePos0, CoolDrive, 0x6061, 0x00, &moddis1, NULL},
    {CoolDrivePos0, CoolDrive, 0x27ff, 0x00, &DB2_1, NULL},
    {CoolDrivePos0, CoolDrive, 0x603f, 0x00, &errcode1, NULL},
    {CoolDrivePos0, CoolDrive, 0x60f4, 0x00, &errflo1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6841, 0x00, &status2, NULL},
    {CoolDrivePos0, CoolDrive, 0x6864, 0x00, &actpos2, NULL},
    {CoolDrivePos0, CoolDrive, 0x6877, 0x00, &acttor2, NULL},    
    {CoolDrivePos0, CoolDrive, 0x6861, 0x00, &moddis2, NULL},
    {CoolDrivePos0, CoolDrive, 0x2fff, 0x00, &DB2_2, NULL},
    {CoolDrivePos0, CoolDrive, 0x683f, 0x00, &errcode2, NULL},
    {CoolDrivePos0, CoolDrive, 0x68f4, 0x00, &errflo2, NULL},
    {CoolDrivePos1, CoolDrive, 0x6041, 0x00, &status3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6064, 0x00, &actpos3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6077, 0x00, &acttor3, NULL},    
    {CoolDrivePos1, CoolDrive, 0x6061, 0x00, &moddis3, NULL},
    {CoolDrivePos1, CoolDrive, 0x27ff, 0x00, &DB2_3, NULL},
    {CoolDrivePos1, CoolDrive, 0x603f, 0x00, &errcode3, NULL},
    {CoolDrivePos1, CoolDrive, 0x60f4, 0x00, &errflo3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6841, 0x00, &status4, NULL},
    {CoolDrivePos1, CoolDrive, 0x6864, 0x00, &actpos4, NULL},
    {CoolDrivePos1, CoolDrive, 0x6877, 0x00, &acttor4, NULL},    
    {CoolDrivePos1, CoolDrive, 0x6861, 0x00, &moddis4, NULL},
    {CoolDrivePos1, CoolDrive, 0x2fff, 0x00, &DB2_4, NULL},
    {CoolDrivePos1, CoolDrive, 0x683f, 0x00, &errcode4, NULL},
    {CoolDrivePos1, CoolDrive, 0x68f4, 0x00, &errflo4, NULL},
    {}
};


/* Master 0, Slave 0, "CoolDrive RD"
 * Vendor ID:       0x00000748
 * Product code:    0x0000000a
 * Revision number: 0x00000002
 */

ec_pdo_entry_info_t slave_0_pdo_entries[] = {
    {0x6040, 0x00, 16},
    {0x607a, 0x00, 32},
    {0x6060, 0x00, 8},
    {0x27fe, 0x00, 8},
    {0x6840, 0x00, 16},
    {0x687a, 0x00, 32},
    {0x6860, 0x00, 8},
    {0x2ffe, 0x00, 8},
    {0x6041, 0x00, 16},
    {0x6064, 0x00, 32},
    {0x6077, 0x00, 16},
    {0x6061, 0x00, 8},
    {0x27ff, 0x00, 8},
    {0x603f, 0x00, 16},
    {0x60f4, 0x00, 32},
    {0x6841, 0x00, 16},
    {0x6864, 0x00, 32},
    {0x6877, 0x00, 16},
    {0x6861, 0x00, 8},
    {0x2fff, 0x00, 8},
    {0x683f, 0x00, 16},
    {0x68f4, 0x00, 32},
};

ec_pdo_info_t slave_0_pdos[] = {
    {0x1600, 4, slave_0_pdo_entries + 0},
    {0x1610, 4, slave_0_pdo_entries + 4},
    {0x1a00, 7, slave_0_pdo_entries + 8},
    {0x1a10, 7, slave_0_pdo_entries + 15},
};

ec_sync_info_t slave_0_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 2, slave_0_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 2, slave_0_pdos + 2, EC_WD_DISABLE},
    {0xff}
};

/* Master 0, Slave 1, "CoolDrive RD"
 * Vendor ID:       0x00000748
 * Product code:    0x0000000a
 * Revision number: 0x00000002
 */

ec_pdo_entry_info_t slave_1_pdo_entries[] = {
    {0x6040, 0x00, 16},
    {0x607a, 0x00, 32},
    {0x6060, 0x00, 8},
    {0x27fe, 0x00, 8},
    {0x6840, 0x00, 16},
    {0x687a, 0x00, 32},
    {0x6860, 0x00, 8},
    {0x2ffe, 0x00, 8},
    {0x6041, 0x00, 16},
    {0x6064, 0x00, 32},
    {0x6077, 0x00, 16},
    {0x6061, 0x00, 8},
    {0x27ff, 0x00, 8},
    {0x603f, 0x00, 16},
    {0x60f4, 0x00, 32},
    {0x6841, 0x00, 16},
    {0x6864, 0x00, 32},
    {0x6877, 0x00, 16},
    {0x6861, 0x00, 8},
    {0x2fff, 0x00, 8},
    {0x683f, 0x00, 16},
    {0x68f4, 0x00, 32},
};

ec_pdo_info_t slave_1_pdos[] = {
    {0x1600, 4, slave_1_pdo_entries + 0},
    {0x1610, 4, slave_1_pdo_entries + 4},
    {0x1a00, 7, slave_1_pdo_entries + 8},
    {0x1a10, 7, slave_1_pdo_entries + 15},
};

ec_sync_info_t slave_1_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 2, slave_1_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 2, slave_1_pdos + 2, EC_WD_DISABLE},
    {0xff}
};


/****************************************************************************/
/****************************************************************************/
int ConfigPDO()
{
    /********************/
    printf("xenomai Configuring PDOs...\n");    
    domainServoOutput = ecrt_master_create_domain(master);
    if (!domainServoOutput) {
        return -1;
    }
    domainServoInput = ecrt_master_create_domain(master);
    if (!domainServoInput) {
        return -1;
    }
    printf("nei cun is ok\n");
    /********************/
    printf("xenomai Creating slave0 configurations...\n");
    sc_cooldrive =
       ecrt_master_slave_config(master, 10,10, 0,0);
    if (!sc_cooldrive ) {
        fprintf(stderr, "Failed to get slave0 configuration.\n");
        return -1;
    }
    else printf("get right slave0 config sc_cooldrive=%d\n",&sc_cooldrive);
    if (ecrt_slave_config_pdos(sc_cooldrive, EC_END, slave_0_syncs)) {
        fprintf(stderr, "Failed to configure slave0 PDOs.\n");
        return -1;
    }
    
    
    /********************/
    /********************
    printf("xenomai Creating slave1 configurations...\n");
    sc_cooldrive =
        ecrt_master_slave_config(master, CoolDrivePos1, CoolDrive);
    if (!sc_cooldrive) {
        fprintf(stderr, "Failed to get slave configuration.\n");
        return -1;
    }
    
    if (ecrt_slave_config_pdos(sc_cooldrive, EC_END, slave_1_syncs)) {
        fprintf(stderr, "Failed to configure PDOs.\n");
        return -1;
    }
    /********************/
    if (ecrt_domain_reg_pdo_entry_list(domainServoOutput, domainServoOutput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    if (ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }

    fprintf(stderr, "Creating SDO requests...\n");

    // ecrt_slave_config_sdo8(sc_estun, 0x6060, 0, 8);
    // ecrt_slave_config_sdo8(sc_estun, 0x60C2, 1, 1);

    return 0;
}


/*****************************************************************************
 * Realtime task
 ****************************************************************************/

void rt_check_domain_state(void)
{
    ec_domain_state_t ds = {};
    ec_domain_state_t ds1 = {};
    //domainServoInput
    ecrt_domain_state(domainServoInput, &ds);
    if (ds.working_counter != domainServoInput_state.working_counter) {
        printf("domainServoInput: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoInput_state.wc_state) {
        printf("domainServoInput: State %u.\n", ds.wc_state);
    }
    domainServoInput_state = ds;
    //domainServoOutput
    ecrt_domain_state(domainServoOutput, &ds1);
    if (ds1.working_counter != domainServoOutput_state.working_counter) {
        printf("domainServoOutput: WC %u.\n", ds1.working_counter);
    }
    if (ds1.wc_state != domainServoOutput_state.wc_state) {
        printf("domainServoOutput: State %u.\n", ds1.wc_state);
    }
    domainServoOutput_state = ds1;
}

/****************************************************************************/
/*****************************************************************************/

void check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding) {
        printf("%u slave(s).\n", ms.slaves_responding);
    }
    if (ms.al_states != master_state.al_states) {
        printf("AL states: 0x%02X.\n", ms.al_states);
    }
    if (ms.link_up != master_state.link_up) {
        printf("Link is %s.\n", ms.link_up ? "up" : "down");
    }

    master_state = ms;
}

/*****************************************************************************/

void check_slave_config_states(void)
{
    ec_slave_config_state_t s;

    ecrt_slave_config_state(sc_cooldrive, &s);

    if (s.al_state != sc_cooldrive_state.al_state) {
        printf("AnaIn: State 0x%02X.\n", s.al_state);
    }
    if (s.online != sc_cooldrive_state.online) {
        printf("AnaIn: %s.\n", s.online ? "online" : "offline");
    }
    if (s.operational != sc_cooldrive_state.operational) {
        printf("AnaIn: %soperational.\n", s.operational ? "" : "Not ");
    }

    sc_cooldrive_state = s;
}

/*****************************************************************************/




int main(void)
{
    printf("hello world!\n");
    master = ecrt_request_master(0);
    if (!master) {
        return -1;
    }
    printf("request_master sucess\n");
    check_master_state();
    //check_slave_config_states();
    ConfigPDO();
    
    
    pause();
    return 0;
}
